/** 
 * @file Device.h
 * 
 * @detail Description : 
 * MSP430 structures and low driver functions
 *
 * @author E. Macias / D. Torres / S. Ravindran
 * @author Texas Instruments, Inc
 * @date December, 2011
 * @version 1.0 - Initial version
* @note Built with IAR Embedded Workbench: 5.20.1 and CCS Version 5.1.0.09000
 **/

#include "msp430.h"
#include "string.h"
#include "math.h"
#include "stdint.h"
#include <stdlib.h>
#include <stdio.h>

#include "types.h"



#ifndef DEVICE_H_
#define DEVICE_H_

//************************ USER MODIFIABLE ***********************************//
#define SAMPLE_FREQ_HZ         50

//*************** Define either AHRS_MODE or CALIBRATION_MODE *****************//
#define AHRS_MODE
//#define CALIBRATION_MODE
//****************************************************************************//

//************************* DO NOT MODIFY ************************************//
#if defined(AHRS_MODE) && defined(CALIBRATION_MODE)
  #error "ERROR: AHRS_MODE and CALIBRATION_MODE can't be enabled at the same time, please #define one of the two modes in <Device.h>." 
#elif defined(AHRS_MODE) || defined(CALIBRATION_MODE)
#else
  #error "ERROR: Neither AHRS_MODE nor CALIBRATION_MODE are enabled, please #define one of the two modes in <Device.h>."  
#endif
//****************************************************************************//

//************************ USER MODIFIABLE ***********************************//
// MSP430 Settings
#define		MHz     			      1000000              // ~1 MHz
#define		SMCLK_FREQ     			     (16*MHz)              // ~16 MHz
#define         XT2CLK_FREQ                          (4*MHz)
//****************************************************************************//


//************************* DO NOT MODIFY ************************************//
// Sub main clock
#define S_MCLK                      0x00
/*! Return value when function is OK */
#define RET_OK  1
/*! Return value when there's an error */
#define RET_ERR 0
//****************************************************************************//

//************************* DO NOT MODIFY ************************************//
// Available USCI_I2C_MODULE
#define USCI_I2C_B0		1
#define USCI_I2C_B1		2
#define USCI_I2C_B2		3
#define USCI_I2C_B3		4

//****************************************************************************//


//************************ USER MODIFIABLE ***********************************//
// F5438A Family: USCI_I2C_B0, USCI_I2C_B1(default), USCI_I2C_B2, USCI_I2C_B3
// F5529  Family: USCI_I2C_B0, USCI_I2C_B1
// G2553  Family: USCI_I2C_B0 
// Choose one of the available USCI_I2C_MODULES 
#define USCI_I2C_MODULE USCI_I2C_B0 

//****************************************************************************//

#include "USCI_I2C_Definitions.h"
#include "USCI_UART_Definitions.h"

#define ENABLE_DCM_TIMER() {TA0CCTL0 |= CCIE;}                 // CCR0 interrupt enabled


// Global Buffers
extern uint8_t global_tx_buffer[32];

#define ToRad(x) (x*0.01745329252)  // *pi/180
#define ToDeg(x) (x*57.2957795131)  // *180/pi
#define M_PI 	 3.141592654

// ADXL345 Sensitivity(from datasheet) => 4mg/LSB   1G => 1000mg/4mg = 256 steps
// Tested value : 256
#define GRAVITY 256  //this equivalent to 1G in the raw data coming from the accelerometer 
#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square

#define Kp_ROLLPITCH 0.0125
#define Ki_ROLLPITCH 0.000008
#define Kp_YAW 1.2
#define Ki_YAW 0.000008

extern float G_Dt;    // Integration time (DCM algorithm)  We will run the integration loop at 200Hz if possible

// IGT-3200 Sensitivity (from datasheet) => 14.375 LSBs//s
// Tested values : 
#define Gyro_Gain_X   14.375 //X axis Gyro gain
#define Gyro_Gain_Y   14.375 //Y axis Gyro gain
#define Gyro_Gain_Z   14.375 //Z axis Gyro gain

// Sensor Variables
struct s_sensor_data
{
    //raw data is uncorrected and corresponds to the
    //true sensor axis, not the redefined platform orientation
    int16_t gyro_x_raw;
    int16_t gyro_y_raw;
    int16_t gyro_z_raw;
    int16_t accel_x_raw;
    int16_t accel_y_raw;
    int16_t accel_z_raw;
    int16_t magnetom_x_raw;
    int16_t magnetom_y_raw;
    int16_t magnetom_z_raw;
  
    //This data has been corrected based on the calibration values
    float gyro_x;
    float gyro_y;
    float gyro_z;
    float accel_x;
    float accel_y;
    float accel_z;
    float magnetom_x;
    float magnetom_y;
    float magnetom_z;
    float magnetom_heading;
    int8_t baro_temp;  
    int32_t baro_pres;
};

//Structure for holding offsets and calibration values for the accel, gyro, and magnetom
struct s_sensor_offsets
{
    float gyro_offset[3];
    float accel_offset[3];
    float magnetom_offset[3];
    float magnetom_XY_Theta;
    float magnetom_XY_Scale;
    float magnetom_YZ_Theta;
    float magnetom_YZ_Scale;
};

typedef enum 
{
    false_state = 0,
    true_state
}boolean_t; 

extern struct s_sensor_data sen_data;
extern struct s_sensor_offsets sen_offset;
extern int16_t SENSOR_SIGN[9];

extern float Accel_Vector[3];    //Store the acceleration in a vector
extern float Mag_Vector[3];      //Store the magnetometer direction in a vector
extern float Gyro_Vector[3];     //Store the gyros turn rate in a vector
extern float Omega_Vector[3];    //Corrected Gyro_Vector data
extern float Omega_P[3];         //Omega Proportional correction
extern float Omega_I[3];         //Omega Integrator
extern float Omega[3];

// Euler angles
extern float roll;
extern float pitch;
extern float yaw;

extern float errorRollPitch[3];
extern float errorYaw[3];


extern float DCM_Matrix[3][3];
extern float Update_Matrix[3][3];
extern float Temporary_Matrix[3][3];

extern void Init_MCU(void);
extern void Sleep(unsigned int delay_ms);


#endif /*DEVICE_H_*/

